#!/usr/bin/python2.7
# -*- coding: utf-8 -*-

"""publish_utils.py,用于协助ros进行消息的发布"""
from re import T
import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
from sensor_msgs.msg import Image, PointCloud2,PointField
from cv_bridge import CvBridge
from std_msgs.msg import Header
import sensor_msgs.point_cloud2 as pcl2



FRAME_ID = 'ars548' #坐标系名称

def publish_camera(cam_publisher, bridge, image):
 cam_publisher.publish(bridge.cv2_to_imgmsg(image, 'bgr8')) #用于把opencv读入的图片转化为ros可以读取的格式
 
def publish_point_cloud(pc_publisher, point_cloud):
 header = Header()
 header.stamp = rospy.Time.now()
 header.frame_id = FRAME_ID  #锚定坐标轴

 pc_publisher.publish(pcl2.create_cloud_xyz32(header, point_cloud[:,:3]))  #这里只发布了点云的xyz坐标


def publish_azimuthLines(ego_car_publisher):
    '''在点云坐标系下发布60度的毫米波水平视野线'''
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()

    marker.id = 0 #标志的唯一ID
    marker.action = Marker.ADD
    marker.lifetime = rospy.Duration() #生命周期
    marker.type = Marker.LINE_STRIP

    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.color.a = 0.5  #透明度
    marker.scale.x = 0.1  #线条宽度

    marker.points = []
    marker.points.append(Point(10, -17.32, 0))
    marker.points.append(Point(0, 0, 0))
    marker.points.append(Point(10, 17.32, 0))

    ego_car_publisher.publish(marker)

def publish_ponitclouds():
    pass
def publish_piontcloud_marker(piontcloud_marker_publisher,id,cloudpoint):
    '''
    in base_link frame publish pointclouds markers
    '''
    marker = Marker()
    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()
    
    marker.id = id #标志的唯一ID
    marker.action = Marker.ADD
    marker.lifetime = rospy.Duration() #生命周期
    marker.type = Marker.SPHERE
    marker.pose.position.x = cloudpoint[0]
    marker.pose.position.y = cloudpoint[1]
    marker.pose.position.z = cloudpoint[2]
    marker.pose.orientation.x = 0.0
    marker.pose.orientation.y = 0.0
    marker.pose.orientation.z = 0.0
    marker.pose.orientation.w = 1.0
    marker.color.r = 5.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.color.a = 1.0  #透明度
    marker.scale.x = 0.4  #线条宽度
    marker.scale.y = 0.4  #线条宽度
    marker.scale.z = 0.4
    # marker.points = []
    # print(cloudpoints)
    # for i in cloudpoints:
    # #     print(type(i[0]))
    #     print(i)
    #     marker.points.append(Point(i[0],i[1],i[2]))
    # marker.points.append(Point(-0, -5, 0))
    # marker.points.append(Point(25, 17.32, 0))
    # marker.points.append(Point(25, 17.32, 2))
    # piontcloud_marker_publisher.publish(marker)
    return marker
def publish_pointclouds(pub,clouds):
    header = Header()
    header.stamp = rospy.Time().now()
    header.frame_id = FRAME_ID

    # point cloud segments
    msg_segment = create_cloud_xyzdr32(header, clouds)  # 点云数据量大，处理的时间就越长

    # publish
    pub.publish(msg_segment)


# create pcl2_msg with 5 fields
def create_cloud_xyzdr32(header, points):
    '''
        Field 其实是自定义的
        fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
        PointField('r', 12, PointField.FLOAT32, 1),
        PointField('g', 16, PointField.FLOAT32, 1),
        PointField('b', 20, PointField.FLOAT32, 1)
    ]
    '''
    fields = [PointField('x', 0, PointField.FLOAT32, 1),
            PointField('y', 4, PointField.FLOAT32, 1),
            PointField('z', 8, PointField.FLOAT32, 1),
            PointField('doppler_v', 12, PointField.FLOAT32, 1),
            PointField('rcs', 16, PointField.FLOAT32, 1)
            ]
    return pcl2.create_cloud(header, fields, points)